function [C] = dcmnormortho_iter(C, stepNum) %#codegen
%DCMNORMORTHO_ITER 对方向余弦矩阵进行规范正交化，按迭代法
%
% Input Arguments:
% # C: 3*3矩阵
% # stepNum: 迭代步数，默认为3
%
% Output Arguments:
% # C: 3*3矩阵，正交化后的方向余弦矩阵
%
% References:
% # 袁信, 郑谔. 捷联式惯性导航原理[M]. 北京: 航空专业教材编审室, 1985

if nargin < 2
    stepNum = 3;
end
% REF1 6-12式
for i = 1:stepNum
    C = 1/2*(inv(C') + C);
end
